# 版权所有 2025 地球仪 (ldhhp@163.com)
# 本文件基于灯哥的 Py-Apple Dog 项目进行修改。
# 原始项目地址：https://github.com/ToanTech/py-apple-quadruped-robot
# 本项目遵循 Apache 许可证 2.0 版。
# 你可以在以下地址获取许可证副本：http://www.apache.org/licenses/LICENSE-2.0

# 机器狗主控制程序，负责运动控制、姿态调节、步态生成等核心功能

#导入狗数据
import dog_data
import dog_cfg
#引入模块
import PA_SERVO
import PA_TROT
import PA_TLT
import PA_IK
import PA_ATTITUDE
import PA_STABLIZE

#import time
import sys_control
from setcfg import DAT

def cal_test_shank(x):
  p1=0.006649
  p2=0.4414
  p3=5.53
  return p1*x*x+p2*x+p3

def servo_output(case,init,leg1_shou,leg1_ham,leg1_shank,leg2_shou,leg2_ham,leg2_shank,leg3_shou,leg3_ham,leg3_shank,leg4_shou,leg4_ham,leg4_shank):
  if init==0:
    #腿1
    PA_SERVO.angle(4,dog_data.parts[0][0]+leg1_shou)
    PA_SERVO.angle(5,dog_data.parts[0][1]+90-(90-leg1_ham))
    PA_SERVO.angle(6,dog_data.parts[0][2]-90+cal_test_shank(-leg1_shank))
    
    #腿2
    PA_SERVO.angle(7,dog_data.parts[1][0]-leg2_shou)
    PA_SERVO.angle(8,dog_data.parts[1][1]-90+(90-leg2_ham))
    PA_SERVO.angle(9,dog_data.parts[1][2]+90-cal_test_shank(-leg2_shank))
    #腿3
    PA_SERVO.angle(10,dog_data.parts[2][0]+leg3_shou)
    PA_SERVO.angle(11,dog_data.parts[2][1]-90+(90-leg3_ham))
    PA_SERVO.angle(12,dog_data.parts[2][2]+90-cal_test_shank(-leg3_shank))
    #腿4
    PA_SERVO.angle(13,dog_data.parts[3][0]-leg4_shou)
    PA_SERVO.angle(14,dog_data.parts[3][1]+90-(90-leg4_ham))
    PA_SERVO.angle(15,dog_data.parts[3][2]-90+cal_test_shank(-leg4_shank))
  else:
    #腿1
    PA_SERVO.angle(4,dog_data.parts[0][0])
    PA_SERVO.angle(5,dog_data.parts[0][1])
    PA_SERVO.angle(6,dog_data.parts[0][2])
    #腿2
    PA_SERVO.angle(7,dog_data.parts[1][0])
    PA_SERVO.angle(8,dog_data.parts[1][1])
    PA_SERVO.angle(9,dog_data.parts[1][2])
    #腿3
    PA_SERVO.angle(10,dog_data.parts[2][0])
    PA_SERVO.angle(11,dog_data.parts[2][1])
    PA_SERVO.angle(12,dog_data.parts[2][2])
    #腿4
    PA_SERVO.angle(13,dog_data.parts[3][0])
    PA_SERVO.angle(14,dog_data.parts[3][1])
    PA_SERVO.angle(15,dog_data.parts[3][2])

def height(goal):    #高度调节函数
    dog_cfg.H_goal=goal

def gesture(PIT,ROL,YAW,X):
    dog_cfg.PIT_goal=PIT
    dog_cfg.ROL_goal=ROL
    dog_cfg.YAW_goal=YAW
    dog_cfg.X_goal=X


def stable(key):
    dog_cfg.key_stab=key
  
def servo_init(key):
    dog_cfg.init_case=key

    
def gait(mode):   #设置步态
  dog_cfg.gait_mode=mode    

run_time=0

def move(spd_,L_,R_,T_=-1):
    global run_time
    if not isinstance(T_, (int, float)):
        print("错误：T_参数必须是数字")
        return
    dog_cfg.spd_goal=spd_
    dog_cfg.Left=L_
    dog_cfg.Right=R_
    run_time=T_*100

cmdlist=["前进", "后退", "左转", "右转", "停止","踏步"]
def hand_cmd(opera,rt):      
  print(f"正在执行：{opera}")
  stable(False)
  gait(0)
  try:
    run_time=rt
    if opera == "前进":
        move(6,1,1)
    elif opera == "后退":
        move(-4,1,1)
    elif opera == "左转":
        move(3,-1,1)
    elif opera == "右转":
        move(3,1,-1)  
    elif opera == "停止":
        move(0,0,0)
    elif opera == "踏步":
        move(0,1,1)        
  except Exception as e:
    print("except:"+str(e))

def name_replay():
    from tts_module_m import play_wav
    play_wav("wangwang.wav")
    

_last_spd_goal=None
_last_spd=None
_last_Left=None
_last_Right=None
_last_P = [0] * 12
_last_PT =[0] * 12
_last_R_H=None
_last_PIT_S=None
_last_ROL_S=None
_last_YAW_S=None
_last_X_S=None 

def mainloop(FPS):
    global run_time,_last_spd_goal,_last_spd,_last_Left,_last_Right,_last_P,_last_PT,_last_R_H,_last_PIT_S,_last_ROL_S,_last_YAW_S,_last_X_S
    try:
      # voltage及异常动作检测
      if sys_control.alarm_and_servo_control()==0:
         PA_SERVO.pca9685_OnOff(1)
      else:
         PA_SERVO.pca9685_OnOff(0)
         return

      # 判断步态模式
      P_ = [0] * 12  # Initialize P_ at the start of the function
      PT_ = [0] * 12  # Initialize P_ at the start of the function
      P_G = [0] * 12  # Initialize P_G at the start of the function
      if run_time>0:
        run_time -=FPS
        if run_time<=0:
            move(0,0,0)
          
      #if _last_spd!=dog_cfg.spd or _last_Left!=dog_cfg.Left or _last_Right!=dog_cfg.Right or _last_spd_goal!=dog_cfg.spd_goal or dog_cfg.spd_goal!=0 or dog_cfg.Left!=0 or dog_cfg.Right!=0:           
      if dog_cfg.gait_mode==0:    #TROT 步态
          if dog_cfg.deltaStep>=(dog_cfg.Ts-dog_data.speed):#一个完整的运动周期结束 trot
              dog_cfg.deltaStep=0
          elif dog_cfg.Left==0 and dog_cfg.Right==0:
              dog_cfg.deltaStep=0
          else:
              val=dog_cfg.deltaStep+dog_data.speed
              if (val>0 and dog_cfg.deltaStep<0) or (val<0 and dog_cfg.deltaStep>0):
                  dog_cfg.deltaStep=0
              else:
                  dog_cfg.deltaStep=val
          
          P_=PA_TROT.cal_t(dog_cfg.deltaStep,0,dog_cfg.spd*8,dog_data.raiseH,dog_cfg.Left,dog_cfg.Left,dog_cfg.Right,dog_cfg.Right)
      elif dog_cfg.gait_mode==2:    #平移步态
          if dog_cfg.deltaStep>=(dog_cfg.Ts-dog_data.speed):#一个完整的运动周期结束 trot
              dog_cfg.deltaStep=0
          elif dog_cfg.Left==0 and dog_cfg.Right==0:
              dog_cfg.deltaStep=0
          else:
              dog_cfg.deltaStep=dog_cfg.deltaStep+dog_data.speed
          P_=PA_TLT.cal_tlt(dog_cfg.deltaStep,0,dog_cfg.spd*4,dog_data.raiseH,dog_cfg.Left,dog_cfg.Left,dog_cfg.Right,dog_cfg.Right)
      #速度调节器
      if dog_cfg.spd>dog_cfg.spd_goal:
          dog_cfg.spd=dog_cfg.spd-abs(dog_cfg.spd-dog_cfg.spd_goal)*dog_cfg.Kp_V
      elif dog_cfg.spd<dog_cfg.spd_goal:
          dog_cfg.spd=dog_cfg.spd+abs(dog_cfg.spd-dog_cfg.spd_goal)*dog_cfg.Kp_V

      _last_spd=dog_cfg.spd;_last_spd_goal=dog_cfg.spd_goal;_last_Left=dog_cfg.Left;_last_Right=dog_cfg.Right
      _last_P=P_[:]
      _last_PT = None
      _last_R_H=None
      _last_PIT_S=None
      _last_ROL_S=None
      _last_YAW_S=None
      _last_X_S=None 
      #else:
      #  P_ = _last_P[:]
    
      
      #if _last_R_H!=dog_cfg.R_H or _last_PIT_S!=dog_cfg.PIT_S or _last_ROL_S!=dog_cfg.ROL_S or _last_YAW_S!=dog_cfg.YAW_S or _last_X_S!=dog_cfg.X_S:
        #高度调节器1
      if dog_cfg.R_H>dog_cfg.H_goal:
          dog_cfg.R_H=dog_cfg.R_H-abs(dog_cfg.R_H-dog_cfg.H_goal)*dog_data.Kp_H
      elif dog_cfg.R_H<dog_cfg.H_goal:
          dog_cfg.R_H=dog_cfg.R_H+abs(dog_cfg.R_H-dog_cfg.H_goal)*dog_data.Kp_H
      #姿态调节器
      if dog_cfg.PIT_S>dog_cfg.PIT_goal:   #俯仰
          dog_cfg.PIT_S=dog_cfg.PIT_S-abs(dog_cfg.PIT_S-dog_cfg.PIT_goal)*dog_data.Kp_G
      elif dog_cfg.PIT_S<dog_cfg.PIT_goal:
          dog_cfg.PIT_S=dog_cfg.PIT_S+abs(dog_cfg.PIT_S-dog_cfg.PIT_goal)*dog_data.Kp_G
      if dog_cfg.ROL_S>dog_cfg.ROL_goal:   #滚转
          dog_cfg.ROL_S=dog_cfg.ROL_S-abs(dog_cfg.ROL_S-dog_cfg.ROL_goal)*dog_data.Kp_G
      elif dog_cfg.ROL_S<dog_cfg.ROL_goal:
          dog_cfg.ROL_S=dog_cfg.ROL_S+abs(dog_cfg.ROL_S-dog_cfg.ROL_goal)*dog_data.Kp_G
          
      if dog_cfg.YAW_S>dog_cfg.YAW_goal:   #偏航
          dog_cfg.YAW_S=dog_cfg.YAW_S-abs(dog_cfg.YAW_S-dog_cfg.YAW_goal)*dog_data.Kp_G
      elif dog_cfg.YAW_S<dog_cfg.YAW_goal:
          dog_cfg.YAW_S=dog_cfg.YAW_S+abs(dog_cfg.YAW_S-dog_cfg.YAW_goal)*dog_data.Kp_G

      if dog_cfg.X_S>dog_cfg.X_goal:   #X位置
          dog_cfg.X_S=dog_cfg.X_S-abs(dog_cfg.X_S-dog_cfg.X_goal)*dog_data.Kp_G
      elif dog_cfg.X_S<dog_cfg.X_goal:
          dog_cfg.X_S=dog_cfg.X_S+abs(dog_cfg.X_S-dog_cfg.X_goal)*dog_data.Kp_G

      #姿态角度限位  
      if dog_cfg.PIT_S>=dog_cfg.pit_max_ang:dog_cfg.PIT_S=dog_cfg.pit_max_ang
      if dog_cfg.PIT_S<=-dog_cfg.pit_max_ang:dog_cfg.PIT_S=-dog_cfg.pit_max_ang
      if dog_cfg.ROL_S>=dog_cfg.rol_max_ang:dog_cfg.ROL_S=dog_cfg.rol_max_ang
      if dog_cfg.ROL_S<=-dog_cfg.rol_max_ang:dog_cfg.ROL_S=-dog_cfg.rol_max_ang
      #TROT模态根据迈腿长度自动调节重心
      if dog_cfg.gait_mode==0:
          if dog_cfg.spd>=0 and (dog_cfg.Left+dog_cfg.Right)!=0:              
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S-abs(dog_cfg.spd)*dog_data.trot_cg_f,dog_cfg.R_H)
          elif dog_cfg.spd<0 and (dog_cfg.Left+dog_cfg.Right)!=0:
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S+abs(dog_cfg.spd)*dog_data.trot_cg_b,dog_cfg.R_H)
          elif (dog_cfg.Left+dog_cfg.Right)==0:    #原地旋转
              P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S+abs(dog_cfg.spd)*dog_data.trot_cg_t,dog_cfg.R_H)
      else:
          P_G=PA_ATTITUDE.cal_ges(dog_cfg.PIT_S,dog_cfg.ROL_S,dog_cfg.YAW_S,dog_data.bodyL,dog_data.bodyW,dog_data.legW,dog_cfg.X_S,dog_cfg.R_H)
      
      #dog_cfg.ges_x_1=P_G[0];dog_cfg.ges_x_2=P_G[1]; dog_cfg.ges_x_3=P_G[2];dog_cfg.ges_x_4=P_G[3]
      #dog_cfg.ges_y_1=P_G[4];dog_cfg.ges_y_2=P_G[5]; dog_cfg.ges_y_3=P_G[6];dog_cfg.ges_y_4=P_G[7]
      #dog_cfg.ges_z_1=P_G[8];dog_cfg.ges_z_2=P_G[9]; dog_cfg.ges_z_3=P_G[10];dog_cfg.ges_z_4=P_G[11]
      PT_ = [P_[i] + P_G[i] for i in range(12)]
      #_last_PT = None
      #_last_R_H=dog_cfg.R_H;_last_PIT_S=dog_cfg.PIT_S;_last_ROL_S=dog_cfg.ROL_S;_last_YAW_S=dog_cfg.YAW_S;_last_X_S=dog_cfg.X_S
      #else:
      #  PT_ = _last_PT[:]

      #自稳调节器（只静态稳定）:
      if abs(0-dog_cfg.spd)<=0.1 and dog_cfg.Left==0 and dog_cfg.Right==0 and dog_cfg.key_stab==True:
          PA_STABLIZE.stab(0)
      
      try:
        #作动
        #if PT_ != _last_PT:
        A_=PA_IK.ik(dog_data.ma_case,dog_data.legL,PT_[0],PT_[1],PT_[2],PT_[3],PT_[4],PT_[5],PT_[6],PT_[7],PT_[8],PT_[9],PT_[10],PT_[11])
        servo_output(dog_data.ma_case,dog_cfg.init_case,A_[0],A_[1],A_[2],A_[3],A_[4],A_[5],A_[6],A_[7],A_[8],A_[9],A_[10],A_[11])
          #_last_PT=PT_[:]
        #sys_control.IK_ERROR=0     
      except Exception as e:
        #sys_control.IK_ERROR=1
        print(f"ik {e}")
    except Exception as e:
      print(f"mainloop {e}")

print("胖胖狗V1.0 智能大脑")
print("开发者:地球仪")
print("作者邮件:ldhhp@163.com")

